#!/usr/bin/env python3

import rospy
from sensor_msgs.msg import Joy
from std_msgs.msg import String
import time

class JoyCtrlNode:
    def __init__(self):
        # 初始化ROS节点
        rospy.init_node('joy_ctrl', anonymous=True)

        # 创建发布者对象，发布到joy_ctrl话题，消息类型为String
        self.pub = rospy.Publisher('joy_ctrl', String, queue_size=10)

        # 创建订阅者对象，订阅joy话题
        self.sub = rospy.Subscriber('joy', Joy, self.joy_callback)

    def map_value(self, value, from_min, from_max, to_min, to_max):
        # 线性映射公式
        return (value - from_min)*(to_max- to_min)/(from_max - from_min) + to_min
    
    def joy_callback(self, joy_msg):
        # 读取参数
        linear_guidewires = joy_msg.axes[4]
        linear_catheter = joy_msg.axes[1]
        angular_guidewires = joy_msg.axes[6]


        # 处理读取的参数
        # 转换为50mm/s,直径d=8mm，°/s
        linear_guidewires_mapped = self.map_value(linear_guidewires, -1.0, 1.0, -716.56, 716.56)
        linear_catheter_mapped = self.map_value(linear_catheter, -1.0, 1.0, -716.56,716.56)
        angular_guidewires_mapped = self.map_value(angular_guidewires, -1.0, 1.0, -716.56, 716.56)


        time.sleep(0.5)

        # 将线速度和角速度转换为字符串格式
        # 例如，使用逗号分隔的字符串 "linear_x,angular_z"
        # motor_command = f"{linear_guidewires_mapped:.2f},{linear_catheter_mapped:.2f},{angular_guidewires_mapped:.2f}\r\n"
        motor_command = [
                         f":0 {linear_catheter_mapped:.2f}\r\n",
                         f":1 {angular_guidewires_mapped:.2f}\r\n",
                         f":2 {linear_guidewires_mapped:.2f}\r\n",
                         f":3 {linear_guidewires_mapped:.2f}\r\n"
                         ]


        # motor_command = f"{linear_guidewires},{linear_catheter},{angular_guidewires}\n"

        # for command in motor_command:
        #     rospy.loginfo("Sending command: %s", command)
        #     self.pub.publish(command)

        # # 创建String消息对象
        
        string_msg = String()
        for command in motor_command:
            string_msg.data = command         
            # 发布String消息到joy_ctrl话题
            self.pub.publish(string_msg)
            rospy.loginfo("Published to joy_ctrl: %s", motor_command)

if __name__ == '__main__':
    try:
        joy_ctrl_node = JoyCtrlNode()
        rospy.spin()
    except rospy.ROSInterruptException:
        pass